/*
@2023-12-04: 更新通过UDP socket给ICS返回消息的功能
@2024-12-03: 增加动态配置读出速度功能，在改变读出速度的同时，还需要更新权重系数；删除部分旧代码
@2024-11-29: 改用UDP
@2024-11-20: 将PL_isMaster功能集成到
@2024-10-23: 基于单芯片相机的socket服务端程序多线程版适配修改后用于2X2拼接相机。该服务端程序运行于4个zynq
             ps端，作为后台程序

TODO:
-[ ] 增加挂载Linux主机工作目录的功能。要实现该功能，需要ICS那边将Linux主机待挂载的目录发送给PS端。 


*/

#include "MephistoCAM.h"

#define PORT            9527

#define IP_ICS          "192.168.0.233"
#define PORT_ICS        10100

#define LISTEN_QUEUE    16
#define BUFFER_SIZE     1024



////////////////////////////////////////////////////////////////////////////////////
// 备份CCD驱动时序参数
// backup vtrans parameters
int vtrans_num_bak; //      = mcfg->config_ccd_drive->vtrans_num;
int imgCLK1_cnt_min_bak;    // = mcfg->config_ccd_drive->imgCLK1_cnt_min;
int imgCLK1_cnt_max_bak;    // = mcfg->config_ccd_drive->imgCLK1_cnt_max;
int imgCLK2_cnt_min_bak;    // = mcfg->config_ccd_drive->imgCLK2_cnt_min;
int imgCLK2_cnt_max_bak;    // = mcfg->config_ccd_drive->imgCLK2_cnt_max;
int imgCLK3_cnt_min_bak;    // = mcfg->config_ccd_drive->imgCLK3_cnt_min;
int imgCLK3_cnt_max_bak;    // = mcfg->config_ccd_drive->imgCLK3_cnt_max;
int imgCLK4_cnt_min_bak;    // = mcfg->config_ccd_drive->imgCLK4_cnt_min;
int imgCLK4_cnt_max_bak;    // = mcfg->config_ccd_drive->imgCLK4_cnt_max;
int imgCLKx_cnt_MAX_bak;    // = mcfg->config_ccd_drive->imgCLKx_cnt_MAX;
int vtrans_state_cnt_max_bak;   // = mcfg->config_ccd_drive->vtrans_state_cnt_max;

// backup htrans parameters
int htrans_num_bak; // = mcfg->config_ccd_drive->htrans_num;
///////////////////////////////////////////////////////////////////////////////////

int extractCommand( const char sock_msg[1024], char *operator, char *operand );

int main( int argc, char *argv[] ) {

    if( argc < 3 ){
        printf("\n*********************************************\n");
        printf("usage: %s band ccd_idx\n", argv[0]);
        printf("*********************************************\n");
        return 0;
    }
    
    int ccd_idx = atoi(argv[2]);
    char myName[64]; // 保存每个CCD的名称Blue1, Blue2, Blue3, Blue4
    sprintf(myName, "%s%d", argv[1], ccd_idx);
    fprintf(stdout, "# My Name is : %s\n", myName);

    char binfilename[1024];
    sprintf(binfilename, "/mnt/tmp/%s.bin", myName);
    
    //=================================================
    //  导入所有配置参数
    //=================================================
    char config_dir[1024];      // 保存参数配置文件所在的目录(相对路径)
    char config_dir_tmp[512];

    FILE *fp_wkdir = fopen(".WKDIR", "r");
    fscanf(fp_wkdir, "%s", config_dir_tmp);
    printf("&&& %s\n", config_dir_tmp);
    fclose(fp_wkdir);

    sprintf(config_dir, "%s_%s", config_dir_tmp, myName);

    printf("### %s\n", config_dir);
/*    exit(0);*/

    MephistoCAM_Config MCAM_Config;
    init_MephistoCAM_Config( &MCAM_Config, config_dir );

    // backup vtrans parameters
    vtrans_num_bak          = MCAM_Config.config_ccd_drive->vtrans_num;
    imgCLK1_cnt_min_bak     = MCAM_Config.config_ccd_drive->imgCLK1_cnt_min;
    imgCLK1_cnt_max_bak     = MCAM_Config.config_ccd_drive->imgCLK1_cnt_max;
    imgCLK2_cnt_min_bak     = MCAM_Config.config_ccd_drive->imgCLK2_cnt_min;
    imgCLK2_cnt_max_bak     = MCAM_Config.config_ccd_drive->imgCLK2_cnt_max;
    imgCLK3_cnt_min_bak     = MCAM_Config.config_ccd_drive->imgCLK3_cnt_min;
    imgCLK3_cnt_max_bak     = MCAM_Config.config_ccd_drive->imgCLK3_cnt_max;
    imgCLK4_cnt_min_bak     = MCAM_Config.config_ccd_drive->imgCLK4_cnt_min;
    imgCLK4_cnt_max_bak     = MCAM_Config.config_ccd_drive->imgCLK4_cnt_max;
    imgCLKx_cnt_MAX_bak     = MCAM_Config.config_ccd_drive->imgCLKx_cnt_MAX;
    vtrans_state_cnt_max_bak= MCAM_Config.config_ccd_drive->vtrans_state_cnt_max;
    
    // backup htrans parameters
    htrans_num_bak          = MCAM_Config.config_ccd_drive->htrans_num;

    //=========================================================================
    // GPIO 
    // uint32_t *gpio_flt_ctrl;        // ADDR_Float_Point_CTRL
    // uint32_t *gpio_flt_data;        // ADDR_Float_Point_DATA
    // uint32_t *gpio_in;              // ADDR_GPIO_IN
    // uint32_t *gpio_pl_ps;           // ADDR_PL_PS_DDR_CNT
    // uint32_t *gpio_out;             // ADDR_GPIO_OUT
    // uint32_t *gpio_out2;            // ADDR_GPIO_OUT
    // uint32_t *gpio_spi_ps_ctrl;     // ADDR_GPIO_SPI_PS_CTRL
    // uint32_t *gpio_spi_ps_data;     // ADDR_GPIO_SPI_PS_DATA


    int fd_gpio = open("/dev/mem", O_RDWR | O_SYNC);

    uint32_t *gpio_flt_ctrl     = mmap(NULL, 10, PROT_READ | PROT_WRITE, MAP_SHARED, fd_gpio, ADDR_Float_Point_CTRL);
    uint32_t *gpio_flt_data     = gpio_flt_ctrl + 2;

    uint32_t *gpio_in           = mmap(NULL, 10, PROT_READ | PROT_WRITE, MAP_SHARED, fd_gpio, ADDR_GPIO_IN);
    uint32_t *gpio_pl_ps        = gpio_in + 2;

    uint32_t *gpio_out          = mmap(NULL, 10, PROT_READ | PROT_WRITE, MAP_SHARED, fd_gpio, ADDR_GPIO_OUT);
    uint32_t *gpio_out2         = gpio_out + 2;

    uint32_t *gpio_spi_ps_ctrl  = mmap(NULL, 10, PROT_READ | PROT_WRITE, MAP_SHARED, fd_gpio, ADDR_GPIO_SPI_PS_CTRL);
    uint32_t *gpio_spi_ps_data  = gpio_spi_ps_ctrl + 2;

    MCAM_Config.gpio_flt_ctrl   = gpio_flt_ctrl;
    MCAM_Config.gpio_flt_data   = gpio_flt_data;
    MCAM_Config.gpio_in         = gpio_in;
    MCAM_Config.gpio_pl_ps      = gpio_pl_ps;
    MCAM_Config.gpio_out        = gpio_out;
    MCAM_Config.gpio_out2       = gpio_out2;
    MCAM_Config.gpio_spi_ps_ctrl= gpio_spi_ps_ctrl;
    MCAM_Config.gpio_spi_ps_data= gpio_spi_ps_data;

    //=================================================
    //      SOCKET
    //=================================================

    ////////////////////////////////////////////////////////////////////////
    // 创建给ICS发送回复的UDP套接字,
    int sock2ics;
    if( (sock2ics = socket(AF_INET,SOCK_DGRAM,0)) < 0 ){
        perror("**** failed creating sock2ics ****\n");
        exit(EXIT_FAILURE);
    }

    // 配置服务端地址
    struct sockaddr_in server_addr_ics;                // 用于给ICS返回消息
    memset(&server_addr_ics, 0, sizeof(server_addr_ics));
    server_addr_ics.sin_family  = AF_INET;
    server_addr_ics.sin_port    = htons(PORT_ICS);

    // 将IP地址转换为网络字节序
    if( inet_pton(AF_INET, IP_ICS, &server_addr_ics.sin_addr) <= 0 ){
        perror("**** failed converting ICS address ****\n");
        close(sock2ics);
        exit(EXIT_FAILURE);
    }
    ////////////////////////////////////////////////////////////////////////

    char recvbuf[BUFFER_SIZE];
    char sendbuf[BUFFER_SIZE];
    char operator[128]   = "";
    char operand[256]    = "";

    //初始化socket
    // int sock_fd = socket(AF_INET,SOCK_STREAM,0);        // TCP
    int sock_fd = socket(AF_INET,SOCK_DGRAM,0);        // UDP
    fcntl(sock_fd, F_SETFL, O_NONBLOCK);
    int on = 1;                                         // Enable address reuse
    setsockopt( sock_fd, SOL_SOCKET, SO_REUSEADDR, &on, sizeof(on) );

    struct sockaddr_in server_sockaddr;                 // socket service
    struct sockaddr_in client_addr;                     // socket client

    socklen_t client_addr_len = sizeof(client_addr);

    memset( &server_sockaddr, 0, sizeof(server_sockaddr));
    bzero( &client_addr, sizeof(client_addr) );

    server_sockaddr.sin_family      = AF_INET;
    server_sockaddr.sin_port        = htons(PORT);
/*    server_sockaddr.sin_addr.s_addr = htonl(INADDR_ANY);*/
    server_sockaddr.sin_addr.s_addr = inet_addr(IP_ICS);
    

    ///bind成功返回0出错返回-1
    if( bind(sock_fd, (struct sockaddr *)&server_sockaddr, sizeof(server_sockaddr)) == -1 )
    {
        fprintf(stderr, "[Mephisto_RED_CAM] Failed to bind socket, quit.\n");
        exit(-1);
    }

    MephistoCAM_Config *mcfg   = &MCAM_Config;
    

    //指向DDR固定地址的指针_old
    
    uint32_t base_addr = 0x20000000;    // 512MByte 特殊地址值！！！！
    uint32_t n_col = mcfg->config_ccd_drive->htrans_num;
    uint32_t n_row = mcfg->config_ccd_drive->vtrans_num;
    uint32_t n_bytes = 2;
    uint32_t mem_size_in_Bytes = n_col*16*n_bytes;   // the actual number of Bytes of a row
    uint32_t *mem_ptr = mmap(NULL, n_row*n_col*16*n_bytes, PROT_READ | PROT_WRITE, MAP_SHARED, fd_gpio, base_addr);
    uint32_t *ptr = mem_ptr;

    /*****************************************************************************************
     *    while循环里包含两类可能得任务：
     *    [1]高优先级的图像数据读出（从DDR中取出，并写入指定路径下、指定文件名的bin文件中。该任务的触发信号来自FPGA
     *    [2]低优先级的“参数配置”任务，具体包括读出速度和采样权重的配置, 行数、列数的配置
     *        -1- 读出速度与采样权重参数通过读取在指定位置处预先保存好的参数文件，指令格式：speed|250k, speed|500k, speed|1000k, speed|2000k
     *        -2- 行数和列数的配置通过UDP socket进行配置， 指令格式:row_size|4700, col_size|1250
     *****************************************************************************************/
    uint32_t row_cnt = 4700;    // row_cnt的默认值不能为0，防止PL端读出状态机静止时BIT_DDR_DATA_RDY=1

    while(1) {

        // 高优先级任务： 在CCD读出的过程中，根据FPGA的信号按顺序将DDR中的数据取出、写入bin文件中
        if( getbit( *(mcfg->gpio_in), BIT_DDR_DATA_RDY) && (row_cnt==0) ){
            ptr = mem_ptr;
            FILE *fb = NULL;
            fb = fopen(binfilename, "wba");   // append mode
            do {
                if( getbit( *(mcfg->gpio_in), BIT_DDR_DATA_RDY) ){
                    fwrite(ptr, sizeof(unsigned char), mem_size_in_Bytes, fb);
                    ptr += n_col*n_bytes*4;
                    row_cnt++;
                }
            } while( row_cnt < n_row );

            if( fb != NULL )
                fclose(fb);

            sprintf(sendbuf, "readoutdone%d", ccd_idx);
            if( sendto(sock2ics, sendbuf, strlen(sendbuf), 0, 
                (struct sockaddr*)&server_addr_ics, sizeof(server_addr_ics)) < 0 ){
                printf("**** failed sending %s to ICS\n", sendbuf);
                close(sock2ics);
                exit(EXIT_FAILURE);
            } else {
                printf(">>>> sucessfully send %s to ICS\n", sendbuf);
            }
        }

        // 检测到BIT_DDR_DATA_RDY变为0时，表示FPGA端的读出状态机再次启动，此时将row_cnt复位至0
        if( getbit( *(mcfg->gpio_in), BIT_DDR_DATA_RDY) == 0 ){
            row_cnt = 0;
        }


        /**********************
         * UDP socket 服务端
         **********************/
        bzero(recvbuf, sizeof(recvbuf));
        bzero(sendbuf, sizeof(sendbuf));

        socklen_t len = recvfrom(sock_fd, recvbuf, sizeof(recvbuf), 0, (struct sockaddr*)&client_addr, &client_addr_len);

        int cmd_set = 0;
        if( len > 0 ){

            fprintf(stdout, "\n**** UDP socket recieved: %s\n", recvbuf);

            extractCommand( recvbuf, operator, operand );
            
            printf("[DEBUG] operator: %s, operand: %s\n", operator, operand);

            if( strcmp(operator, "speed") == 0 ){
            
                printf("fuck speed\n");
                
                printf("[DEBUG] operator: %s, operand: %s\n", operator, operand);

/*                fprintf(fp_log, "> ready to configure readout speed ICS\n");*/

                // 先通过system系统调用函数直接调用固定路径下的脚本完成配置任务
                char cmd1[1024];
                char cmd2[1024];

                if( strcmp(operand, "250k") == 0 ){
/*                    fprintf(fp_log, "# chosen 250kHz\n");*/
                    printf("# chosen 250kHz\n");

                    sprintf(cmd1, "./bin/ccd290_drive_config params_250K_mosaic_blue%d/ccd_drv.ini", ccd_idx);
                    sprintf(cmd2, "./bin/dcds_w_cfg params_250K_mosaic_blue%d/dcds_weights_cfg.ini", ccd_idx);
                    cmd_set = 1;
                }

                if( strcmp(operand, "500k") == 0 ){
/*                    fprintf(fp_log, "# chosen 500kHz\n");*/
                    printf("# chosen 500kHz\n");
                    sprintf(cmd1, "./bin/ccd290_drive_config params_500K_mosaic_blue%d/ccd_drv.ini", ccd_idx);
                    sprintf(cmd2, "./bin/dcds_w_cfg params_500K_mosaic_blue%d/dcds_weights_cfg.ini", ccd_idx);
                    cmd_set = 1;
                }

                if( strcmp(operand, "1000k") == 0 ){
/*                    fprintf(fp_log, "# chosen 1000kHz\n");*/
                    printf("# chosen 1000kHz\n");

                    sprintf(cmd1, "./bin/ccd290_drive_config params_900K_mosaic_blue%d/ccd_drv.ini", ccd_idx);
                    sprintf(cmd2, "./bin/dcds_w_cfg params_900K_mosaic_blue%d/dcds_weights_cfg.ini", ccd_idx);
                    cmd_set = 1;
                }

                if( strcmp(operand, "2000k") == 0 ){
/*                    fprintf(fp_log, "# chosen 2000kHz\n");*/
                    printf("# chosen 2000kHz\n");

                    sprintf(cmd1, "./bin/ccd290_drive_config params_2000K_mosaic_blue%d/ccd_drv.ini", ccd_idx);
                    sprintf(cmd2, "./bin/dcds_w_cfg params_2000K_mosaic_blue%d/dcds_weights_cfg.ini", ccd_idx);
                    cmd_set = 1;
                }

                printf("run cmd1: %s\n", cmd1);
                if( cmd_set ){
                    system(cmd1);
                }
                
                printf("run cmd2: %s\n", cmd2);
                if( cmd_set ){
                    system(cmd2);
                }

                memset(sendbuf, 0, sizeof(sendbuf));
                sprintf(sendbuf, "speeddone%d", ccd_idx);
                
/*                fprintf(fp_log, "> ready to send speeddone%d to ICS\n", ccd_idx);*/
                printf("> ready to send speeddone%d to ICS\n", ccd_idx);

                if( sendto(sock2ics, sendbuf, strlen(sendbuf), 0, 
                    (struct sockaddr*)&server_addr_ics, sizeof(server_addr_ics)) < 0 ) {
                    printf("**** failed sending %s to ICS\n", sendbuf);
                    close(sock2ics);
                    exit(EXIT_FAILURE);
                } else {
                    printf(">>>> sucessfully send %s to ICS\n", sendbuf);
                }

/*                fprintf(fp_log, "> finished sending speeddone%d to ICS\n", ccd_idx);*/

            }
            else if( strcmp(operator, "readout") == 0 ){
                printf("received readout\n");
                memset(sendbuf, 0, sizeof(sendbuf));
                sprintf(sendbuf, "readoutdone%d", ccd_idx);
                if( sendto(sock2ics, sendbuf, strlen(sendbuf), 0, 
                    (struct sockaddr*)&server_addr_ics, sizeof(server_addr_ics)) < 0 ){
                    printf("**** failed sending %s to ICS\n", sendbuf);
                    close(sock2ics);
                    exit(EXIT_FAILURE);
                } else {
                    printf(">>>> sucessfully send %s to ICS\n", sendbuf);
                }
            }
            else {
                fprintf(stdout, "\n***** unresolved command: %s ******\n", operator);
            }

            memset(recvbuf, 0,sizeof(recvbuf));
            memset(sendbuf, 0, sizeof(sendbuf));
            memset(operator, 0, sizeof(operator));
            memset(operand, 0, sizeof(operand));
        }
        // usleep(1000);   // 1ms sleep
    }

    close(sock_fd);

    return 0;
}


int extractCommand( const char sock_msg[1024], char *operator, char *operand ){
    char local_str[1024];
    strcpy(local_str, sock_msg);
    
    char *p;
    sprintf(operator, "%s", strtok(local_str, "|"));
    // fprintf(stdout, "##> operator: --%s--\n", operator);
    
    p = strtok(NULL, "|");

    if( p != NULL ){
        sprintf(operand, "%s", p);
        // fprintf(stdout, "##> operand:  --%s--\n", operand);
    }

    return 0;
}
